#include "Chassis.h"
#include "myInit.h"

struct DirectionControl 
{
    float Angle_1, Angle_2, Angle_3, Angle_4;  
											   
    int8_t sign;  
};

static float Angle_Min(float X, float Y) 
{
    float Z1 = Y - X;
    while(Z1 < -180.0f) 
		Z1 += 360;
    while(Z1 > 180.0f)
		Z1 -= 360;
    return Z1;
}

static float Angle_Optimize(float Z) 
{
    if (Z > 90) return -(180 - Z);
    else if (Z < -90) return 180 + Z;
    return Z;
}

void ControlDirection(struct DirectionControl *ctrl, float* M, int16_t* N) 
{
	ctrl->Angle_4 = Angle_Min(ctrl->Angle_2, *M);  
	ctrl->Angle_3 = ctrl->Angle_1 + Angle_Optimize(ctrl->Angle_4);  
	
	if (ctrl->Angle_4 > 90.0f || ctrl->Angle_4 < -90.0f)  
		ctrl->sign = -(ctrl->sign);  
	
	ctrl->Angle_1 = ctrl->Angle_3; 
	ctrl->Angle_2 = *M;  
	
	if (ctrl->sign==-1)  
		*N = -*N;  
	
	*M = ctrl->Angle_3;  
}

struct DirectionControl Wheel_1 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_2 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_3 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_4 = {0, 0, 0, 0, 1};

void M_dir_ct_Four_plus(float *M_LB_A, int16_t *M_LB_V,
                        float *M_LF_A, int16_t *M_LF_V,
                        float *M_RB_A, int16_t *M_RB_V,
                        float *M_RF_A, int16_t *M_RF_V) 
{
    ControlDirection(&Wheel_1, M_LB_A, M_LB_V);
    ControlDirection(&Wheel_2, M_LF_A, M_LF_V);
    ControlDirection(&Wheel_3, M_RB_A, M_RB_V);
    ControlDirection(&Wheel_4, M_RF_A, M_RF_V);
}

void Delay(uint32_t Delaytime)
{
	osDelay(Delaytime);
}

/*------计算自转速度------*/
#define Kp 8        // 7
#define Kd 0.2        // 0.1
float Calculate_W(float ComputerAng_DIFF,float CoordAng_DIFF,float sYaw_Aim
					,float Pass_Ang,float sYaw_Ang,uint8_t W_Sign) 
{
	static float sW = 0;
	static float CoordAng_DIFF_Next = 0;
	static float CoordAng_DIFF_Last = 0;
	static float sW_Last = 0;
									
	static uint16_t sStopCount = 0;
	
	switch (W_Sign)
	{
		case  0:  	// 边走边瞄
					sW  = Kp*(CoordAng_DIFF - CoordAng_DIFF_Next)
							+Kd*(CoordAng_DIFF-2*CoordAng_DIFF_Next+CoordAng_DIFF_Last);
					CoordAng_DIFF_Last = CoordAng_DIFF_Next;
					CoordAng_DIFF_Next = CoordAng_DIFF;
					sW = sW_Last + sW;
					sW_Last = sW;
					break;
		
		case  1:	// 相机瞄准
					sW = 7*ComputerAng_DIFF;
					if(fabs(ComputerAng_DIFF)<3.5)
						sW =  8*ComputerAng_DIFF;
					if(ComputerAng_DIFF>0&&ComputerAng_DIFF<1.2)
					{
						sW =  5;
					}
					if(ComputerAng_DIFF<0&&ComputerAng_DIFF>-1.2)
					{
						sW = -5;
					}
					if(fabs(ComputerAng_DIFF) < 0.26)  // 相机死区
					{
						sW = 0;
					}
						
					break; 	
					
		case  2:    // 雷达瞄准(单片机算)
					sW = 7*CoordAng_DIFF;
					if(fabs(CoordAng_DIFF)<3.5)
						sW = 8*CoordAng_DIFF;	
					if(CoordAng_DIFF>0&&CoordAng_DIFF< 1.2)
						sW =  5;
					if(CoordAng_DIFF<0&&CoordAng_DIFF> -1.2)
						sW = -5;
					if(fabs(CoordAng_DIFF) < 0.2)  // 雷达死区
						sW = 0;
					break; 

		case  3:    // 雷达瞄准(小电脑算)
					sW = -7*ComputerAng_DIFF;
					if(fabs(ComputerAng_DIFF)<3.5)
						sW = -8*ComputerAng_DIFF;
					if(ComputerAng_DIFF>0&&ComputerAng_DIFF<1.2)
						sW = -5;
					if(ComputerAng_DIFF<0&&ComputerAng_DIFF>-1.2)
						sW =  5;
					if(fabs(ComputerAng_DIFF) < 0.5)  // 
						sW = 0;
					break; 

		case  4:    // 保持瞄好的角度
						sW = 0;
					break; 		
		
		case 5:     // 保持正方向
					sW = -7*sYaw_Ang;
					if(sYaw_Ang>0&&sYaw_Ang< 1.85)
						sW =  -5;
					if(sYaw_Ang<0&&sYaw_Ang>-1.85)
						sW =   5;
					if(fabs(sYaw_Ang) < 0.45)  // 死区
						sW = 0;
					break; 
		case 6:		// 传球瞄准	
					sW = 7*Pass_Ang;
					if(fabs(Pass_Ang)<3.5)
						sW = 8*Pass_Ang;
					if(Pass_Ang>0&&Pass_Ang< 1.2)
						sW =  5;
					if(Pass_Ang<0&&Pass_Ang>-1.2)
						sW = -5;
					if(fabs(Pass_Ang) < 0.25)  // 雷达死区
						sW = 0;	
					break;
		case 7:     
					sW = 0;
					break;
		default:	break;	
	}
	
	if(sW>500)
		sW=500;
	if(sW<-500)
		sW=-500;
	
	return sW;
}

/*------底盘控制------*/
void Helm_Wheel_Control(int16_t Vx,int16_t Vy,float W,int8_t W_Sign,uint8_t sBlock_Sign)
{		
	static int16_t V_F,V_L,V_B,V_R = 0;
	
	static float angle_F,angle_L,angle_R,angle_B = 0.f;
	static MotorToCAN sMotorToCAN;	
	
	/*---解算---*/
	
	if(W == 0)
		W = 0.00001;
	
	angle_F = atan2f((Vy+W),(Vx)); 
	angle_B = atan2f((Vy-W),(Vx)); 
	angle_L = atan2f((Vy),(Vx-W));
	angle_R = atan2f((Vy),(Vx+W));

	V_F = sqrt((Vy+W)*(Vy+W)+(Vx)*(Vx));
	V_B = sqrt((Vy-W)*(Vy-W)+(Vx)*(Vx)); 
	V_L = sqrt((Vy)*(Vy)+(Vx-W)*(Vx-W));
	V_R = sqrt((Vy)*(Vy)+(Vx+W)*(Vx+W));
	
	angle_F = My_RadToAng(angle_F);
	angle_B = My_RadToAng(angle_B); 
	angle_L = My_RadToAng(angle_L);
	angle_R = My_RadToAng(angle_R);

	M_dir_ct_Four_plus(&angle_F, &V_F,
					   &angle_B, &V_B, 
					   &angle_L, &V_L,
					   &angle_R, &V_R);
	/*---驱动---*/
	sMotorToCAN.ID = HelmFID;
	sMotorToCAN.Data = angle_F;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmBID;
	sMotorToCAN.Data = angle_B;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmLID;
	sMotorToCAN.Data = angle_L;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmRID;
	sMotorToCAN.Data = angle_R;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	osDelay(1);
	if(sBlock_Sign > 0 && (W_Sign == 4 || W_Sign == 7) )
	{	
		sMotorToCAN.ID = WheelFID;
		sMotorToCAN.Data =  0;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelBID;
		sMotorToCAN.Data =  0;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelLID;
		sMotorToCAN.Data =  0;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRID;
		sMotorToCAN.Data =  0;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	}else
	{
		sMotorToCAN.ID = WheelFID;
		sMotorToCAN.Data =  V_F;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelBID;
		sMotorToCAN.Data =  -V_B;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelLID;
		sMotorToCAN.Data =  V_L;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRID;
		sMotorToCAN.Data =  -V_R;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	}
}


